/*
 * Copyright (C) 2020 1km Group Holding Limited
 */

#include "ulog/ulog.h"
#include "aos/kernel.h"
#include "stdio.h"
#include "board.h"

#include "can_open.h"

#define CO_MASTER_CORE_EVENT_PARAM_MAX_LEN 32
#define CO_MASTER_EVENT_QUEUE_SIZE         64

#define CO_MASTER_CORE_STACK_SIZE 2048
#define CO_SLAVE_NODE_COUNT	10

static aos_queue_t canopen_master_core_queue;

static uint8_t schedule_inited = 0;

static uint8_t *canopen_core_queue_buffer = NULL;

static uint8_t canopen_master_initd = 0;

static const can_node_id_t node_id = { 1, 1};

//定义各个从站的站号
static const uint8_t canopen_dev_node_list[CO_SLAVE_NODE_COUNT] = {1, 2, 3, 4, 5, 
																   6, 7, 8, 9, 10};
static bool is_xp_canope_master_opened = 0;


static int32_t canopen_master_notify(const co_master_core_event_t type, const uint8_t len, const void *param);

/**
 * @brief 接收相应CAN端口的从站RPDOn的数据，根据RPDOn进行处理，用户可以注册回调函数或数据
 * 
 * @param rx_data 接收到的从站RPDOn的数据
 * @param user_data 用户注册的回调函数或数据
 */
static void user_pdo_rx(const pdo_rx_cb_para_t *rx_data, void *user_data)
{
    if (NULL != rx_data) {
        LOGD(CANOPEN_MASTER_LOG_TAG, "Handle user rcv (%d %d) PDO %d len %d data[0x%x 0x%x 0x%x 0x%x 0x%x 0x%x 0x%x 0x%x]",
            rx_data->port, rx_data->node, rx_data->idx, rx_data->len,
            rx_data->data[0], rx_data->data[1], rx_data->data[2], rx_data->data[3],
            rx_data->data[4], rx_data->data[5], rx_data->data[6], rx_data->data[7]);

			//TODO:这里触发RPDO的处理行为，后根据canopen设备实时数据的内容去定义相应的行为
    }
}

/**
 * @brief 从站状态改变触发的回调函数，可以注册状态改变调用的函数或数据。
 * 
 * @param node 底层传上来的关于主站用的CAN口与从站的站号
 * @param state 从节点状态
 * @param user_data 注册状态改变调用的函数或数据
 */
static void user_components_state_change(const can_node_id_t *node, const nmt_state_t state, void *user_data)
{
    if (NULL != node && NULL != user_data) {
        LOGD(CANOPEN_MASTER_LOG_TAG, "Port %d Node (0x%x) State Change to %d", node->port, node->node, state);

		//在从站初始化状态改变后可以进行相应的处理，例如在预操作模式下操作SDO之类
		//后面根据要访问的canopen设备来进行处理
        switch (state)
        {
        case nmt_state_pre_operational:
        {
			//co_master_core_sdo关于SDO的配置被删除，只剩下让从站进入操作模式的操作。
			canopen_master_notify(co_master_core_sdo, sizeof(node->node), &node->node);
			printf("Slave Node :%d, entry nmt_state_pre_operational\n", node->node);
        }
        break;

        case nmt_state_operational:
      
			printf("Slave Node :%d, entry nmt_state_operational\n", node->node);
            break;

        case nmt_state_init:
    
			printf("Slave Node :%d, entry nmt_state_init\n", node->node);
            break;

        default:
            break;
        }

    }
}

/**
 * @brief 检测事件并处理
 * 
 * @param param 线程任务参数，没用到
 */
static void canopen_master_routine(void *param)
{
    char *event_tmp = (char*)aos_malloc(sizeof(co_master_core_event_header_t) + CO_MASTER_CORE_EVENT_PARAM_MAX_LEN);

    if (NULL != event_tmp) {
        unsigned int rcv_size = 0;
        co_master_core_event_header_t *event_header = (co_master_core_event_header_t*)event_tmp;

        while (1) {
            if (0 == aos_queue_recv(&canopen_master_core_queue, AOS_WAIT_FOREVER, event_tmp, &rcv_size)) {
                switch (event_header->event)
                {
                case co_master_core_reset_all: //重启重站，主站用到的CAN端口需要指定。
                    co_node_reset_all(PORT_CAN_RESERVED);

                    break;

                case co_master_core_sdo: //操作SDO的事件，主站用到的CAN端口需要指定。
                    if (event_header->length == sizeof(uint8_t)) {
                        uint8_t i = 0;
                        uint8_t *pnode = (uint8_t *)&event_tmp[sizeof(co_master_core_event_header_t)];

#if CO_MASTER_SKIP_OD_ACCESS
                        bool od_access_completely = true;
                        LOGI(CANOPEN_MASTER_LOG_TAG, "Skip SDO Access, directly going to operation");
#else

#endif				   
                        /* make component enter into operation */
                        if (0 != co_node_enter_operation(PORT_CAN_RESERVED, *pnode)) {
                            LOGE(CANOPEN_MASTER_LOG_TAG, "MAKE Node Enter Operation Fail", *pnode);
                        }

                    } else {
                        LOGW(CANOPEN_MASTER_LOG_TAG, "invalid length of input(%d)", event_header->length);
                    }

                    break;

                default:
                    LOGE(CANOPEN_MASTER_LOG_TAG, "Unknown Event %d(%d)", event_header->event, rcv_size);
                    break;
                }
            }
        }

        aos_free(event_tmp);
        event_tmp = NULL;
    } else {
        LOGE(CANOPEN_MASTER_LOG_TAG, "allocate event tmp fail");
    }
}

/**
 * @brief 初始化主站事件消息队列并启动任务检测事情
 * 
 * @return int32_t 0成功，非0表示失败
 */
static int32_t canopen_master_core_init(void)
{
    int rc = -1;

    if (0 == schedule_inited) {
        const uint16_t queue_size = (sizeof(co_master_core_event_header_t) + CO_MASTER_CORE_EVENT_PARAM_MAX_LEN)*CO_MASTER_EVENT_QUEUE_SIZE;

        canopen_core_queue_buffer = (uint8_t*)aos_malloc(queue_size);
        if (NULL != canopen_core_queue_buffer) {
            if (0 == aos_queue_new(&canopen_master_core_queue, canopen_core_queue_buffer, queue_size,
                sizeof(co_master_core_event_header_t) + CO_MASTER_CORE_EVENT_PARAM_MAX_LEN)) {
                if (0 != aos_task_new("co_master", canopen_master_routine, NULL, CO_MASTER_CORE_STACK_SIZE)) {
                    LOGE(CANOPEN_MASTER_LOG_TAG, "create routine fail");
                } else {
                    schedule_inited = 1;
                    rc = 0;
                }
            } else {
                LOGE(CANOPEN_MASTER_LOG_TAG, "create queue fail");
            }

            if (0 != rc) {
                aos_free(canopen_core_queue_buffer);
                canopen_core_queue_buffer = NULL;
            }
        }
    }
    return rc;
}

/**
 * @brief 发送SDO事件队列消息进行复位以及与从站的SDO操作
 * 
 * @param type 事件类型
 * @param len 消息长度
 * @param param 消息内容
 * @return int32_t 0成功，非0表示失败
 */
static int32_t canopen_master_notify(const co_master_core_event_t type, const uint8_t len, const void *param)
{
    int32_t rc = -1;
    if (1 == schedule_inited && len <= CO_MASTER_CORE_EVENT_PARAM_MAX_LEN) {
        char tmpbuf[CO_MASTER_CORE_EVENT_PARAM_MAX_LEN + sizeof(co_master_core_event_header_t)];
        co_master_core_event_header_t *p = tmpbuf;

        p->event = type;
        p->length = len;

        if (NULL != param && len > 0) {
            memcpy(&tmpbuf[sizeof(co_master_core_event_header_t)], param, len);
        }
        rc = aos_queue_send(&canopen_master_core_queue, tmpbuf, sizeof(co_master_core_event_header_t) + len);
    }

    return rc;
}

/**
 * @brief canopen主站初始化
 * 
 * @param can_port CAN端口
 * @param node_list 从站个数
 * @param node_cnt 从站站号列表
 * @return int32_t 0成功，非0表示失败
 */
static int32_t canopen_master_init(const uint8_t can_port, const uint8_t *node_list, const uint8_t node_cnt)
{
    int32_t rc = -1;

    if (NULL != node_list && node_cnt > 0 && 0 == canopen_master_initd) {

		//初始化canopen主站
        if (0 == canopen_init(can_port, CANOPEN_MASTER, NULL)) {
            LOGI(CANOPEN_MASTER_LOG_TAG, "start canopen");

			//可以修改为从站状态改变时触发的操作，内部应该只是该CAN口注册有多少个从站节点
			int user_components_state = 0;
			if (node_install(can_port, node_cnt, node_list, user_components_state_change, &user_components_state) != 0) {
                LOG("Node Install fail");
            } else {
                rc = 0;
                LOGI(CANOPEN_MASTER_LOG_TAG, "register pdo");
                uint8_t i = 0;
                for (; i < node_cnt; i++) {

					//为不同的从站注册pdo的回调函数，这里主要是rpdo的处理
                    pdo_register_cb(can_port, node_list[i], user_pdo_rx, NULL);

					//加载作为相应从站的sdo访问客户端
                    if (0 != sdo_install(can_port, node_list[i], co_sdo_role_client, NULL, 0)) {
                        LOGE(CANOPEN_MASTER_LOG_TAG, "install sdo failed");
                    }
                }

                aos_msleep(100);
            }
        } else {
            LOGE(CANOPEN_MASTER_LOG_TAG, "start canopen fail");
        }
        canopen_master_initd = 1;
    }

    return rc;
}

/**
 * @brief canopen设备初始化
 * 
 * @param can_port CAN端口
 * @param node_cnt 从站个数
 * @param node_list 从站站号列表
 * @return int32_t 0成功，非0表示失败
 */
static int32_t canopen_dev_manager_init(const uint8_t can_port, const uint8_t node_cnt, const uint8_t *node_list)
{   
    canopen_master_core_init();

    if (0 != canopen_master_init(can_port, node_list, node_cnt)) {
        LOGE(CANOPEN_MASTER_LOG_TAG, "co master init fail");
        return -1;
    }

	//初始化后重启所有从站
    canopen_master_notify(co_master_core_reset_all, 0, NULL);

    return 0;
}

/**
 * @brief xp canopen主站设备初始化
 * 
 * @return int32_t 0成功，-1表示初始化失败，-2表示canopen设备已经注册了
 */
int32_t xp_canopen_dev_manager_init()
{   
	if(1 == is_xp_canope_master_opened){
		
		LOGE(CANOPEN_MASTER_LOG_TAG, "xp canopen master already open");
		return -2;
	}

	if(0 != canopen_dev_manager_init(PORT_CAN_RESERVED, CO_SLAVE_NODE_COUNT, canopen_dev_node_list)){
		return -1;
	}
	
	is_xp_canope_master_opened = 1;
	return 0;
}



